/**
 * @file /include/weeding_robot_gui/qnode.hpp
 *
 * @brief Communications central!
 *
 * @date 2021.7.21
 **/
/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef weeding_robot_gui_QNODE_HPP_
#define weeding_robot_gui_QNODE_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

// To workaround boost/qt4 problems that won't be bugfixed. Refer to
//    https://bugreports.qt.io/browse/QTBUG-22829
#ifndef Q_MOC_RUN
#include <ros/ros.h> 
#endif
#include <string>
#include <QThread>
#include <QStringListModel>
#include <QLabel>
#include <QImage>
#include <QSettings>
#include <map>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>   //image_transport
#include <cv_bridge/cv_bridge.h>              //cv_bridge
#include <opencv4/opencv2/highgui/highgui.hpp>
#include <sensor_msgs/image_encodings.h>    //图像编码格式
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Float32.h>

//#include "ui_main_window.h"
#include <weed_segment/weed_geometry_msgs.h>
/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace weeding_robot_gui {

/*****************************************************************************
** Class
*****************************************************************************/

class QNode : public QThread {
    Q_OBJECT
public:
    QNode(int argc, char** argv );
    virtual ~QNode();
    bool init();
    bool init(const std::string &master_url, const std::string &host_url);
    void run();

    void move_base(char k,float speed_linear,float speed_trun);

    /*********************
    ** Logging
    **********************/
    enum LogLevel {
        Debug,
        Info,
        Warn,
        Error,
        Fatal
    };

    QStringListModel* loggingModel() { return &logging_model; }
    void log( const LogLevel &level, const std::string &msg);

    void subImage(QString topic, int frame_id);
    void subImageCallback_1(const sensor_msgs::ImageConstPtr& msg);
    void subImageCallback_2(const sensor_msgs::ImageConstPtr& msg);
    void subImageCallback_3(const sensor_msgs::ImageConstPtr& msg);
    void subImageCallback_4(const sensor_msgs::ImageConstPtr& msg);

Q_SIGNALS:
    void loggingUpdated();
    void rosShutdown();

    void showImage(int,QImage);

private:
    int init_argc;
    char** init_argv;
    ros::Publisher chatter_publisher;
    ros::Publisher cmd_pub;
    QStringListModel logging_model;

    image_transport::Subscriber image_sub0;
    image_transport::Subscriber image_sub1;
    image_transport::Subscriber image_sub2;
    image_transport::Subscriber image_sub3;

    QImage Mat2QImage(cv::Mat const& src);
};

}  // namespace weeding_robot_gui

#endif /* weeding_robot_gui_QNODE_HPP_ */
